#include "kinematics.h"
#include <geometry_msgs/Twist.h>
#include <fox_msgs/car_cmd.h>
#include <fox_msgs/car_data.h>

#include "ros/ros.h"
std::vector<double> current_motors_speed(3);
geometry_msgs::Twist expect_vel;
void RobotDataCallBack(const fox_msgs::car_data msg){
    for (size_t i = 0; i < current_motors_speed.size(); i++)
    {
        current_motors_speed[i] = msg.speed[i];
    }
    
}
void RobotVelCallBack(const geometry_msgs::Twist::ConstPtr &msg){
    expect_vel = *msg;
}
int main(int argc, char** argv)
{
    ros::init(argc, argv, "bobac_kinematics");
    reinovo::ReinovoKinematics rk;
    ros::NodeHandle nh;
    std::string twist_topic;
    ros::NodeHandle p_nh("~");//private NodeHandle, for parameter set
    ros::Subscriber status_sub = nh.subscribe<fox_msgs::car_data>("car_data", 1, RobotDataCallBack); //
    ros::Publisher motor_speed_pub = nh.advertise<fox_msgs::car_cmd>("car_cmd", 10);
    ros::Publisher real_vel_pub = nh.advertise<geometry_msgs::Twist>("real_vel", 10);
    if(!p_nh.getParam("twist_topic", twist_topic)) {
        twist_topic = "cmd_vel";
        ROS_INFO("use default param \"twist_topic = %s\"",twist_topic.c_str());
    } else {
        ROS_INFO("twist_topic = %s", twist_topic.c_str());
    }
    ros::Subscriber vel_sub = nh.subscribe<geometry_msgs::Twist>(twist_topic.c_str(), 1, RobotVelCallBack); //subscribe topic = "cmd_vel"

    //get kinematics_mode
    if(!p_nh.getParam("kinematics_mode", rk.car_data_.car_mode)) {
        rk.car_data_.car_mode = rk.kNone;
        ROS_INFO("no kinematics_mode set");
        return 0;
    } else {
        ROS_INFO("kinematics_mode = %d", rk.car_data_.car_mode);
    }
    //get wheel_diameter and wheel_separation
    if(!p_nh.getParam("wheel_radius", rk.car_data_.wheel_radius)) {
        rk.car_data_.wheel_radius = default_wheel_radius;
        ROS_INFO("use default param \"wheel_radius = %g\"",rk.car_data_.wheel_radius);
    } else {
        ROS_INFO("wheel_radius = %g", rk.car_data_.wheel_radius);
    }
    if(!p_nh.getParam("wheel_separation_x", rk.car_data_.wheel_separation_x)) {
        rk.car_data_.wheel_separation_x = 0;
        ROS_INFO("use default param \"wheel_separation_x = %g\"",rk.car_data_.wheel_separation_x);
    } else {
        ROS_INFO("wheel_separation_x = %g", rk.car_data_.wheel_separation_x);
    }
    if(!p_nh.getParam("wheel_separation_y", rk.car_data_.wheel_separation_y)) {
        rk.car_data_.wheel_separation_y = 0;
        ROS_INFO("use default param \"wheel_separation_y = %g\"",rk.car_data_.wheel_separation_y);
    } else {
        ROS_INFO("wheel_separation_y = %g", rk.car_data_.wheel_separation_y);
    }
    //get max velocity
    if(!p_nh.getParam("max_vx", rk.car_data_.max_x_vel)) {
        rk.car_data_.max_x_vel = default_vx;
        ROS_INFO("use default param \"max_vx = %g\"",rk.car_data_.max_x_vel);
    } else {
        ROS_INFO("max_vx = %g", rk.car_data_.max_x_vel);
    }

    if(!p_nh.getParam("max_vy", rk.car_data_.max_y_vel)) {
        rk.car_data_.max_y_vel = default_vy;
        ROS_INFO("use default param \"max_vy = %g\"",rk.car_data_.max_y_vel);
    } else {
        ROS_INFO("max_vy = %g", rk.car_data_.max_y_vel);
    }

    if(!p_nh.getParam("max_vth", rk.car_data_.max_theta_vel)) {
        rk.car_data_.max_theta_vel = default_vth;
        ROS_INFO("use default param \"max_vth = %g\"",rk.car_data_.max_theta_vel);
    } else {
        ROS_INFO("max_vth = %g", rk.car_data_.max_theta_vel);
    }
    ros::Rate loop(20);
    while(ros::ok()){
        ros::spinOnce();
        geometry_msgs::Twist real_vel_msgs;
        std::vector<double> real_vel_temp;
        real_vel_temp  = rk.ForwardKinematics(current_motors_speed);
        real_vel_msgs.linear.x = real_vel_temp[0];
        real_vel_msgs.linear.y = real_vel_temp[1];
        real_vel_msgs.angular.z = real_vel_temp[2];
        real_vel_pub.publish(real_vel_msgs);
        fox_msgs::car_cmd car_cmd_msgs;
        car_cmd_msgs.speed =rk.InverseKinematics(expect_vel.linear.x, expect_vel.linear.y, expect_vel.angular.z);
        motor_speed_pub.publish(car_cmd_msgs);
        loop.sleep();
    }
    return 0;
}